#ifndef CARTOGRAPHER_ADAPTIVE_MATCHER_H_
#define CARTOGRAPHER_ADAPTIVE_MATCHER_H_

#include <deque>
#include <algorithm>
#include <cmath>
#include <functional>
#include <limits>

#include "Eigen/Core"
#include "Eigen/Geometry"

#include "cartographer/mapping/2d/grid_2d.h"
#include "cartographer/mapping/internal/2d/scan_matching/correlative_scan_matcher_2d.h"
#include "cartographer/mapping/proto/scan_matching/real_time_correlative_scan_matcher_options.pb.h"
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/math.h"
#include "cartographer/mapping/2d/probability_grid.h"
#include "cartographer/mapping/internal/2d/tsdf_2d.h"
#include "cartographer/sensor/point_cloud.h"
#include "cartographer/transform/transform.h"
#include "glog/logging.h"
#include "cartographer/rcm/time.h"

using namespace cartographer::mapping;
using namespace cartographer::mapping::scan_matching;

namespace cartographer
{
    typedef mapping::scan_matching::proto::RealTimeCorrelativeScanMatcherOptions ScanMatchOptions;

    struct LinearBounds
    {
        LinearBounds()
        {
        }

        LinearBounds(int min_xp, int max_xp, int min_yp, int max_yp)
            : min_x(min_xp)
            , max_x(max_xp)
            , min_y(min_yp)
            , max_y(max_yp)
        {
        }

        LinearBounds(const LinearBounds &ot)
            : min_x(ot.min_x)
            , max_x(ot.max_x)
            , min_y(ot.min_y)
            , max_y(ot.max_y)
        {
        }

        int min_x;
        int max_x;
        int min_y;
        int max_y;
    };

    double GaussProbability(const Eigen::Vector3d &delta, const Eigen::Vector3d &sigma);

    Eigen::Vector3d ComputeOffset(const transform::Rigid2d &pose_init, const transform::Rigid2d &pose_opt);

    float ComputeCandidateScore(const TSDF2D &tsdf, const DiscreteScan2D &discrete_scan, int x_index_offset, int y_index_offset);

    float ComputeCandidateScore(const ProbabilityGrid &probability_grid, const DiscreteScan2D &discrete_scan, int x_index_offset,
                                int y_index_offset);

    float ComputeRangeMax(const sensor::PointCloud &cloud, const float resolution);

    void PrintCandidate(const char *tag, const Candidate2D &cand);

    bool CompareScorePair(std::pair<int, int> a, std::pair<int, int> b);

    void NormalizeWeight(std::vector<double> &weights, const double weight_sum);

    std::vector<sensor::PointCloud> GenerateRotatedScans(const sensor::PointCloud &cloud, const std::vector<double> &rotations);

    struct HistogramGroup
    {
        int id = -1;
        int count = 0;
        double value = 0.0;
    };

    class CandidateScoreComputor2D
    {
    public:
        CandidateScoreComputor2D(const mapping::Grid2D &grid2d);

        float Compute(const sensor::PointCloud &rotated_cloud, int scan_index, const double x, const double y);

        const mapping::Grid2D &grid2d_;
    };

    class PoseApi2d
    {
    public:
        PoseApi2d()
        {
            pose.setZero();
        }

        PoseApi2d(const transform::Rigid2d &init)
        {
            Set(init);
        }

        inline void Set(const transform::Rigid2d &init)
        {
            pose(0) = init.translation().x();
            pose(1) = init.translation().y();
            pose(2) = init.rotation().cast<float>().angle();
        }

        inline transform::Rigid3f Rotation()
        {
            return transform::Rigid3f::Rotation(Eigen::AngleAxisf(pose(2), Eigen::Vector3f::UnitZ()));
        }

        inline Eigen::Translation2f Translation()
        {
            return Eigen::Translation2f(pose(0), pose(1));
        }

        inline transform::Rigid2d GetPose(const Candidate2D &cand)
        {
            const auto rotation = Eigen::Rotation2Dd(pose(2)) * Eigen::Rotation2Dd(cand.orientation);
            return transform::Rigid2d({pose(0) + cand.x, pose(1) + cand.y}, rotation);
        }

        Eigen::Vector3f pose;
    };

    class GaussComputer
    {
    public:
        GaussComputer()
        {
        }

        inline void Push(float yaw)
        {
            while (hisYaw_.size() > 9)
                hisYaw_.pop_front();
            hisYaw_.push_back(yaw);
            inited_ = false;
            if (hisYaw_.size() > 9)
            {
                float total = 0;
                float size = float(hisYaw_.size());
                for (float val : hisYaw_)
                    total += val;
                average = total / size;

                total = 0;
                for (float val : hisYaw_)
                    total += std::pow(average - val, 2.0);
                conv = total / size;
                deviation = std::sqrt(conv);
                inited_ = true;
            }
        }

        inline void Clear()
        {
            while (hisYaw_.size() > 0)
                hisYaw_.pop_front();
            inited_ = false;
        }

        inline double Prop(const double curr, const double expv)
        {
            if (inited_)
                return (1 / (deviation * std::sqrt(2 * M_PI))) * std::exp(-0.5 * std::pow((curr - average) / deviation, 2.0));
            else
                return std::exp(-common::Pow2(std::abs(curr - expv)));
        }

        bool inited_ = false;
        float conv = 0.0;
        float average = 0.0;
        float deviation = 0.0;
        std::deque<float> hisYaw_;
    };
} // namespace cartographer

#endif